/**
  ****************************(C) COPYRIGHT 2019 DJI****************************
  * @file       servo_task.c/h
  * @brief      
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Oct-21-2019     RM              1. done
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2019 DJI****************************
  */

#include "servo_task.h"
#include "main.h"
#include "cmsis_os.h"
#include "bsp_servo_pwm.h"
#include "remote_control.h"
#include "robot_total_mode_task.h"
#include "robot_config.h"
#include "OLED.h"
#include "debug_config.h"
#include "referee.h"
#include "bsp_led.h"

static const remote_control_t* remoteControl_p; //图传链路控制

#define SERVO_OPEN_PWM   950   
#define SERVO_CLOSE_PWM   2600    

#define PWM_DETAL_VALUE 10

#define SERVO_OPEN_KEY  KEY_PRESSED_OFFSET_B
#define SERVO_CLOSE_KEY  KEY_PRESSED_OFFSET_CTRL

static const RC_ctrl_t* rc_p;
int target_pwm = SERVO_OPEN_PWM;


void servo_task(void const * argument)
{
    rc_p = get_remote_control_point();
    remoteControl_p=get_usart_remote_control_p();
    while(1)
    {
        if(key_has_changed()){
            if(remoteControl_p->keyboard_value&BULLET_BASE_OPEN){
                #ifdef LED_SERVO
                aRGB_led_show(0xFF00FF00);
                #endif
                target_pwm = SERVO_OPEN_PWM;
            }
            else if(remoteControl_p->keyboard_value&BULLET_BASE_CLOSE){
                #ifdef LED_SERVO
                aRGB_led_show(0xFFFF0000);
                #endif
                target_pwm = SERVO_CLOSE_PWM;
            }
        }else{
            if(switch_is_up(rc_p->rc.ch[RC_CHOOSE_Y_OR_W])){
                #ifdef LED_SERVO
                aRGB_led_show(0xFF00FF00);
                #endif
                target_pwm = SERVO_OPEN_PWM;
            }
            else if(switch_is_down(rc_p->rc.ch[RC_CHOOSE_Y_OR_W])){
                #ifdef LED_SERVO
                aRGB_led_show(0xFFFF0000);
                #endif
                target_pwm = SERVO_CLOSE_PWM;
            }
        }
        for(uint8_t i = 0; i < 7; i++)
        {
            servo_pwm_set(target_pwm,i);
        }
        osDelay(10);
    }
}

void OLED_servo(void)
{
    // OLED_printf(4,4,"0:%d,1:%d,2:%d,3:%d",servo_pwm[0],servo_pwm[1],servo_pwm[2],servo_pwm[3]);
    OLED_printf(4,4, "pwm: %d", target_pwm);
}

